//
// Created by xiang on 2022/7/18.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "ch7/direct_ndt_lo.h"
#include "ch7/ndt_3d.h"
#include "common/dataset_type.h"
#include "common/io_utils.h"
#include "common/timer/timer.h"
#include "common/sensor_calib.h"
#include "tools/ui/pangolin_window.h"
#include "ch7/imu_process.h"
#include "ch7/eskf_filter.h"
#include "ch7/quaternion_kinematics.h"

DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");
//DEFINE_string(pcd_path, "../data/ch7/map.pcd", "点云文件路径");
DEFINE_string(pcd_path, "", "点云文件路径");
DEFINE_bool(use_pcl_ndt, false, "use pcl ndt to align?");
DEFINE_bool(use_ndt_nearby_6, false, "use ndt nearby 6?");
DEFINE_bool(display_map, false, "display map?");
DEFINE_double(antenna_angle, 0.0, "RTK天线安装偏角（角度）");
DEFINE_double(antenna_pox_x, -0.25, "RTK天线安装偏移X");
DEFINE_double(antenna_pox_y, 0.0, "RTK天线安装偏移Y");
DEFINE_bool(with_ui, true, "是否显示图形界面");
DEFINE_bool(with_odom, true, "是否加入轮速计信息");
DEFINE_bool(with_gps, false, "是否gps信息");
DEFINE_bool(with_points, false, "是否points信息");

using PointType = pcl::PointXYZI;
using PointCloudType = pcl::PointCloud<PointType>;

int main(int argc, char** argv) {
    if (!FLAGS_pcd_path.empty()) {
        // 读取点云
        PointCloudType::Ptr cloud(new PointCloudType);
        pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);

        if (cloud->empty()) {
            LOG(ERROR) << "cannot load cloud file";
            return -1;
        }
        // visualize
        pcl::visualization::PCLVisualizer viewer("map cloud viewer");
        pcl::visualization::PointCloudColorHandlerGenericField<PointType> handle(cloud, "z");  // 使用高度来着色
        viewer.addPointCloud<PointType>(cloud, handle);
        viewer.spin();
    }

    ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }

    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    sad::DirectNDTLO::Options options;
    options.use_pcl_ndt_ = fLB::FLAGS_use_pcl_ndt;
    options.ndt3d_options_.nearby_type_ =
        FLAGS_use_ndt_nearby_6 ? sad::Ndt3d::NearbyType::NEARBY6 : sad::Ndt3d::NearbyType::CENTER;
    options.display_realtime_cloud_ = FLAGS_display_map;
    sad::DirectNDTLO ndt_lo(options);

    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);
    Scan2dPtr last_scan = nullptr, current_scan = nullptr;
    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
    bool imu_inited = false, gnss_inited = false;
    /// 设置各类回调函数
    bool first_gnss_set = false;
    Vec3d origin = Vec3d::Zero();
    ImuProcess imu_process;

    //sad::EskFilter eskf;
    sad::QuaternionKinematics eskf;
    
    SensorCalib sensor_calib;
    PointCloudType local_map;
    SE3 pose;
    SE3 predict_pose;

    static sad::NavState<double> last_odom_state;

    ndt_lo.SetImuToLidarExt(sensor_calib.EigenToSE3(sensor_calib.GetLeftVelodynePoseInImu()));

    io.SetIMUProcessFunc([&](const sad::IMU& imu) {
          /// IMU 处理函数
          if (!imu_process.InitSuccess()) {
              imu_process.AddIMU(imu);
              return;
          }

          /// 需要IMU初始化
          if (!imu_inited) {
              // 读取初始零偏，设置ESKF
              //sad::EskFilter::Options quat_options;
              sad::QuaternionKinematics::Options quat_options;

              // 噪声由初始化器估计
              quat_options.gyro_var_ = std::sqrt(imu_process.GetCovGyro()[0]);
              quat_options.acce_var_ = std::sqrt(imu_process.GetCovAcce()[0]);
              eskf.SetInitialConditions(quat_options, imu_process.GetInitBg(), imu_process.GetInitBa(), imu_process.GetGravity(), imu_process.GetRotation());
              imu_inited = true;
              return;
          }

          /// GNSS 也接收到之后，再开始进行预测
          eskf.Predict(imu);
         
          /// predict就会更新ESKF，所以此时就可以发送数据
          auto state = eskf.GetNominalState();
          if (FLAGS_with_ui) {
              ui.UpdateNavState(state);
          }
          
          usleep(1e3);
    })         
        .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
            /// GNSS 处理函数
            if (!imu_inited) {
                return;
            }

            sad::GNSS gnss_convert = gnss;
            if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
                return;
            }
            /// 去掉原点
            if (!first_gnss_set) {
                origin = gnss_convert.utm_pose_.translation();
                first_gnss_set = true;
            }
            gnss_convert.utm_pose_.translation() -= origin;
            if(FLAGS_with_gps) {
                // 要求RTK heading有效，才能合入ESKF
                eskf.ObserveGps(gnss_convert);
                auto state = eskf.GetNominalState();
                if (FLAGS_with_ui) {
                    ui.UpdateNavState(state);
                }                
            }

            gnss_inited = true;
    })
        .SetOdomProcessFunc([&](const sad::Odom& odom) {
            /// Odom 处理函数，本章Odom只给初始化使用
            imu_process.AddOdom(odom);
            if(!FLAGS_with_gps) {
                if(FLAGS_with_odom && imu_inited) {
                    //eskf.ObserveWheelSpeed(odom);
                    eskf.ObserveWheelPoseSpeed(odom, last_odom_state);

                    return;
                }
            }
            if (FLAGS_with_odom && imu_inited && (gnss_inited)) {
                //eskf.ObserveWheelSpeed(odom);
                eskf.ObserveWheelPoseSpeed(odom, last_odom_state);
            }
            last_odom_state = eskf.GetNominalState();
    })
        .SetPointCloud3DProcessFunc([&](const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_ptr) -> bool  {

            common::Timer::Evaluate(
                [&]() {
                    if (!FLAGS_with_points || !imu_inited || (FLAGS_with_gps && !gnss_inited)) {
                        return false;
                    }    
                    
                    //predict_pose = sensor_calib.ImuToLidarMotion(eskf.GetNominalSE3(), sensor_calib.GetLeftVelodynePoseInImu());
                    predict_pose = eskf.GetNominalSE3();
                    //predict_pose = SE3();
                    static int frame_num = 0;
                    frame_num++;
                    float voxel_size = 0.1;
                    if(frame_num > 30) {
                        if(eskf.GetVelocity() > 0.00001) {
                            ndt_lo.AddCloud(sad::VoxelCloud(cloud_ptr, voxel_size), pose, predict_pose);
                        }
                    } else {
                        ndt_lo.AddCloud(sad::VoxelCloud(cloud_ptr, voxel_size), pose, predict_pose);
                    }

                    //eskf.ObserveSE3(pose, 0.01, 0.01);

                    if(ui.ShouldQuit() == false) {
                        if(!ndt_lo.local_map()->empty()) {
                            local_map = *ndt_lo.local_map();

                            //ui.UpdateNavState(sad::NavStated(0, pose));
                            //ui.UpdateScan(sad::VoxelCloud(cloud_ptr, voxel_size), SE3(SO3(), Eigen::Matrix<double, 3, 1>::Zero()));
                            //ui.UpdateScan(sad::VoxelCloud(cloud_ptr, voxel_size), ndt_lo.world_pose());

                            //ui.UpdateScan(local_map.makeShared(), pose);
                            ui.UpdateScan(local_map.makeShared(), SE3());
                        }
                    }
                },
                "NDT registration");
            return true;
    })
    .Go();

    if (FLAGS_display_map) {
        // 把地图存下来
        ndt_lo.SaveMap("../data/ch7/map.pcd");
    }

    common::Timer::PrintAll();
    LOG(INFO) << "done.";

    ui.Quit();

    return 0;
}
